import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.robotics.navigation.Move;
import lejos.robotics.subsumption.Behavior;

/**
 * Highest priority class to be able to turn off the robot. Also tells
 * BillManager to load the live data.
 * @see BillManager
 */
public class FaceGoal implements Behavior {

	private boolean suppressed = false;
	private int targetGoalInDegrees = 0;
	private int IROffset = 110;

	/**
	 * Shutdown.
	 */
	public void action() {

		int previousSpeed_A = Motor.A.getSpeed();
		int previousSpeed_B = Motor.B.getSpeed();

		while (!isFacingCorrectDirection() && hasBall() && !suppressed) {
			if (!Motor.A.isMoving() && !Motor.B.isMoving()) {
				Motor.A.setSpeed(50);
				Motor.B.setSpeed(50);

				Motor.A.forward();
				Motor.B.backward();
			}
		}

		Motor.A.stop();
		Motor.B.stop();

		Motor.A.setSpeed(previousSpeed_A);
		Motor.B.setSpeed(previousSpeed_B);

		if (isFacingCorrectDirection())
			BillManager.SetIsFacingGoal(true);

		//while ((Motor.A.isMoving() || Motor.B.isMoving()) && !suppressed)
			//Thread.yield();
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see lejos.robotics.subsumption.Behavior#suppress()
	 */
	public void suppress() {
		suppressed = true;
	}

	/**
	 * Only take control if we have the ball.
	 */
	public boolean takeControl() {
		if (isFacingCorrectDirection())
			BillManager.SetIsFacingGoal(true);

		return hasBall();
	}

	/**
	 * Calculates if the robot is facing the correct direction.
	 * @return true if correct direction, false if not correct direction.
	 */
	private Boolean isFacingCorrectDirection() {
		// calculates the difference between the current compass degree value and
		// the value that the robot needs to look at
		float difference = Math.abs(BillManager.getCompassData() - targetGoalInDegrees);
		if (difference > 180)
			difference = Math.abs(difference - 360);

		// The accepted difference in degree value
		float acceptedDifference = 20f;

		// robot is facing the target with a accepted difference
		if (difference < acceptedDifference)
			return true;

		return false;
	}

	/**
	 * Do we have the ball?
	 * @return True if the ball is close, otherwise false.
	 */
	public boolean hasBall() {
		return ((BillManager.GetInfraredData() == 5) && (BillManager
				.GetInfraredSensor3Data() > IROffset));
	}
}
